#ifndef __CONTROL_H
#define __CONTROL_H
#include "PID.h"
#include "AGV.h"
#include "oled.h"

extern uint32_t THRESHOLD;    //阈值
extern uint32_t targetSpeed;  //Unit: mm/s
extern double actrulPosition; //Unit: cm
extern uint32_t angleKp;
extern uint32_t angleKi;
extern uint32_t angleKd;

extern uint8_t runMode; //0停止 1运行

extern uint8_t AGV_Data[8];

extern uint32_t FinalTime;

//转向环使能
#define ANGLECYCLE 1

//电机控制
void rMotorControl(int32_t rmOutput);
void lMotorControl(int32_t lmOutput);
void servoControl(int32_t dir);

void runInit(void);
void justrun(void);
void stateDisplay(void);

//上位机
/******************************************************************/
#define BYTE0(Temp) (*(char *)(&Temp))       //0-7位
#define BYTE1(Temp) (*((char *)(&Temp) + 1)) //8-15位
#define BYTE2(Temp) (*((char *)(&Temp) + 2)) //16-23位
#define BYTE3(Temp) (*((char *)(&Temp) + 3)) //24-31位
/******************************************************************/
void Send_Variable(float *Variable, uint8_t Variable_num);
#endif
